#include "includes.h"


char temp[10];
int16_t speed;


void motor_init()
{
	// init kvadraturteller og motor
	MJ1_EN = 0xFF; 		// sets MJ1 port output
	MJ2_EN = 0x00; 		// sets MJ2 port input
	set_bit(MJ1,OE);	// sets !OE high	
	
	set_bit(MJ1,EN);	// enable motor
}

void calculate_motor(int joystick)
{
	unsigned char messageBuf[4];				// twi_message shell
	messageBuf[0] = TWI_targetSlaveAddress;		// twi_message slave address
	messageBuf[1] = 0x00;						// twi_message command

	if(joystick>20) {
		set_bit(MJ1,DIR);						// Sets direction
		messageBuf[2] = 120+(joystick)/10;
	}
	else if(joystick<-20) {
		clear_bit(MJ1,DIR);						// Sets direction
		messageBuf[2] = 120+(abs(joystick))/10;
	}
	else {
		messageBuf[2] = 0x00;
	}

	TWI_Start_Transceiver_With_Data(messageBuf,3);	
}

void regulator(int joystick)
{
	unsigned char messageBuf[4];				// twi_message shell
	messageBuf[0] = TWI_targetSlaveAddress;		// twi_message slave address
	messageBuf[1] = 0x00;						// twi_message command

	if(joystick>0) {
		set_bit(MJ1,DIR);						// Sets direction
		messageBuf[2] = (joystick-kvadraturteller());
	}
	else if(joystick<0) {
		clear_bit(MJ1,DIR);						// Sets direction
		messageBuf[2] = ((abs(joystick))-abs(kvadraturteller()));
	}
	else {
		messageBuf[2] = 0x00;
	}

	TWI_Start_Transceiver_With_Data(messageBuf,3);	
}

int16_t kvadraturteller()
{
	int16_t avlest = 0;

	clear_bit(MJ1,OE);							// Clears !OE
	clear_bit(MJ1,SEL);							// Clears SEL

	_NOP;
	_NOP;

	avlest = (MJ2 & 0x0F)<<12;

	set_bit(MJ1,SEL);							// Sets SEL

	_NOP;
	_NOP;

	avlest |= MJ2<<4;

	clear_bit(MJ1,RST);							// Clears Reset
	set_bit(MJ1,RST);							// Sets Reset
	set_bit(MJ1,OE);							// Sets !OE

	return ((avlest)>>4);
}

